package nxt.WallRobo_03;
/**
 * !!!VERALTET!!! ROBOTER UMGEBAUT
 * WallRobo Version 0.3.2 Zuletzt ge�ndert von Zafar am 16.3.2011 19:09
 *	
 *	Version 0.3.2: [16.3.2011 17:48] {Getestet}
 *		-Statics rausgemacht
 *		-Klasse Main in Controller umbenannt
 *		-Constructor in controller eingefügt
 *
 *
 * 	Version 0.3.1.: [16.3.2011 15:22] {Getestet}
 * 		- 250DEG/s bzw. 17,45 cm/s ASpeed
 * 		- Wanddriftberechnung mit last und current distance
 * 		- Autostop bei Zurueckgelegte Strecke < Abweichung
 * 		- 20 DEG Correctionwinkel 
 * 		- Threads: Main, Usthread, Correctionthread, Athread
 */

////FORMELN\\\\ 
//
//Radstand=11,3cm
//Radradius=4cm
//-1*Motor=US
//-43,3276*Steer=US
//0,02308*Motor=Steer
//Drift in ° = asin(Abweichung/Zurückgelegte Strecke) * - Wallside
//Abweichung = distance - defaultdistance 
//Zurückgelegte Strecke = (Tacho(A)*Radumfang)/-360
//Radius(Wendekreis) = Radstand/ sin(Vorderradeinschlag)
//Standard Correctionwinkel : 30°
//
//
//

import lejos.nxt.*;
public class Controller
{	
	public Correctionthread ct;
	public Athread at;
	public Usthread ust;
	public UltrasonicSensor us;
	public static void main(String [] args) throws Exception 
	{
		Controller controller = new Controller();
	}
	
	public Controller()
	{
		us = new UltrasonicSensor(SensorPort.S1);
		Motor.B.setSpeed(500);
		Motor.A.setSpeed(250);
		Motor.B.setPower(100);
		Sound.setVolume(100);
		

		ust = new Usthread();
		ust.start();
		
		try
		{
			synchronized(ust.wait_for_wallside)
			{
				ust.wait_for_wallside.wait();
			}
		} catch(Exception e){}
		at = new Athread();
		at.start();
					
		
		while(!Button.ENTER.isPressed())
		{
			Motor.B.rotateTo(90);
				
			Sound.playTone(2000,100);
			Motor.B.stop();
			Motor.B.rotateTo(-90);
				
			Sound.playTone(2000,100);
			Motor.B.stop();
			Motor.B.rotateTo(0);
				
			Sound.playTone(2000,100);
			Motor.B.stop();
			
//Correctionpoint

			if (ust.correction_is_needed==true)
			{
				
				ct = new Correctionthread();
				ct.start();
				try
				{
					synchronized(Correctionthread.wait_for_correction)
					{
						Correctionthread.wait_for_correction.wait();
					}
				} catch(Exception e){}
				ust.correction_in_progress = false;
				
			}
			
			Motor.B.rotateTo(-90);
				
			Motor.B.stop();
			Motor.B.rotateTo(90);
				
			Motor.B.stop();
			Motor.B.rotateTo(0);
				
			Motor.B.stop();
			
//Correctionpoint

			if (ust.correction_is_needed==true)
			{
				ct = new Correctionthread();				
				ct.start();
				try
				{
					synchronized(Correctionthread.wait_for_correction)
					{
						Correctionthread.wait_for_correction.wait();
					}
				} catch(Exception e){}

				ust.correction_in_progress = false;
				
			}
		}	
		
	Motor.A.stop();
	Motor.B.rotateTo(0);
	System.exit(0);
	
	}
	
}
